.NEWSS MOTIONS
.NEWSSS	COMPILE-TIME AND RUNTIME CONSIDERATIONS

	All motion statements  cause the compiler to make  some plans
for  the eventual execution of  the motion.  These  plans are more or
less complicated, depending  on the exact  type of motion  requested.
Those motions which depend on the  value of some frames as parameters
to the  action will be planned using the compile-time planning values
for all relevant frames.  

	Immediately before the arm starts moving on a trajectory, the
plan is modified to bring it into line with current values of frames.
The result of this last-minute  modification is that if there is  any
discrepancy  between the  runtime and  compile-time understanding  of
where  any frame is, the servo will place  the arm in the right place
nonetheless.  There are limits to the proper use  of this feature; if
the planning value is seriously  in error (and this can mean anywhere
from a few inches to a foot, depending on the arm being used and  its
configuration), then the attempt to make last-minute corrections will
either overstrain the arm or impair force and free sensing (discussed
below).

     It  is  the   user's  responsibility   to  foresee   such
discrepancies in the planning value and to branch her program so that
several  moves  are planned  (with  ASSERT statements  to  inform the
compiler of the  assumptions being used). The  IF-THEN-ELSE statement
will  be useful in  performing the  correct branch  at runtime.   Its
condition will most likely involve the location of a frame.
Actually, the compiler will eventually be able to do some of this
artificial splitting by using locus information and given tolerances.

	The last  step  of any  motion  is  the reevaluation  of  the
location of the hand, and the  updating, if necessary,  of the values
of all frames attached to it.

.NEWSSS	SIMPLE MOVES

	A move is simple if it involves only one  arm. 
A smooth
trajectory is  compiled by
splining   together  polynomial   segments  (usually   third  degree,
occasionally fourth)  separately for each arm joint.  This trajectory
calculation is somewhat  time-consuming,  and  is done completely  at
compile time.
The arm is expected to travel from its  current position
to the  final position,   passing through any  specified intermediate
positions.   The standard way  to avoid the table  is to begin motion
directly away from it and to end motion directly toward it.  There is
a default offset associated  with each frame,  called its "deproach",
which  is  used  to  calculate  the   first  and  the  last  of   the
intermediate, or "via" points.  The deproach of a frame  is stored in
the assertional data base, which is discussed in {sssref asr}.
	An example:
.UNFILL
	MOVE YELLOW			α{Smooth motion, for yellow armα}
		TO FROBGRASP		α{Name of (frame) destinationα}
		VIA SWING1, SWING2	α{Two via-pointsα}
.REFILL
This example demonstrates the general syntax; the reserved
word MOVE is followed by the name of the frame to be moved (usually an
arm) and a set of clauses, each beginning with a
reserved word (here the words TO and VIA).  There is no punctuation
necessary at the end of a clause.

In the example above,
The first implicit via point will be  the deproach point for whatever
frame  at which  the yellow  arm is currently  positioned.   The last
implicit via point will be the deproach point for frobgrasp.

	At  each  of  the via-points,    several  conditions  may  be
specified.  These  are velocity and upper or lower bounds on the time
required to reach this frame from the previous one on the list. Also,
one may specify that a  piece of  code is to  be initiated when a VIA
point is achieved; this is done with a THEN construct.  The statement
following THEN may not be  a jump or a motion statement for  the same
arm.  (If the statement is a motion statement, it must be surrounded
by BEGIN and END.)
	An  example:  
.UNFILL
	VIA  F1 (VEL=3*Z), F2 THEN ENABLE  "CH1",  F3 (VEL=0, DURATION=5)    .
.REFILL
This specifies three via points.  At the first, the velocity is to be
3*Z, at  the second a scalar variable is to  be set, and at the third
the velocity is to  be 0 and  it should take 5  seconds to get  there
from F2.

	Certain things must be specified for any  move.  First is the
arm  which is to be  moved.  It  can be named directly  (as YELLOW or
BLUE) or by naming a frame which is to be moved: If FROB  is attached
to a hand,   it is perfectly  reasonable to request that  the frob be
moved  to a particular location.   So if FROB is  a frame attached to
BLUE, then both FROB and BLUE are "controllable frames"; MOVE FROB is
perfectly legal. A discussion of frame attachment can be found in 
{ssref att}.

	Next, the destination  frame must be  specified. TO F1  means
that at the end  of the motion, the controllable frame  (assume it is
an  arm) should coincide with  the frame F1.   MOVE FROB  TO F2 means
that at the end of the motion,  FROB coincides with F2.  A notational
convenience about  destinations: They  can be  specified in  terms of
where  the controllable  frame is at  the start  of the  motion.  The
symbol for this is ⊗.   That is,  ⊗ is a frame which  is the location
and orientation of the controllable frame at the start of the motion.
Thus, ⊗ +  (0,0,1)  is a frame  1 unit above  the starting place  (in
table coordinates).

.NEWSSS	OPTIONS FOR MOVES

	DIRECTLY tells the compiler that only the via points and the
final point are of interest; no smooth trajectory need be planned.
A smooth motion will result due to runtime calculations.
This will also set the deproaches to NIL.
(Deproaches are discussed in {sssref dep}.)

	ON requests that certain conditions  be continually monitored
during motion.   These can be conditions  of any sort, including flag
checking,   force checking,   and time  checking.   If any  monitored
condition triggers,  the DO part associated with it will be executed.
The  "block" of  a motion-based  ON monitor  is the  motion statement
itself; exiting the motion will disable the test.  

	Several functions  can be  tested continually; these  include
force along a vector (FORCE(V)), time since beginning 
of motion
(DURATION), and
the force between the fingers (SQUEEZE).

	One more "function" is testable: ARRIVAL.   This becomes true
when  the  motion   terminates  due  to  either  having  reached  its
destination.
It does not become true if the arm stops for reasons other than normal
arrival at the destination; STOP does not trigger it.
	An example: 
.UNFILL
	ON FORCE(Z)>10 DO
		BEGIN WRITE("OUCH"); STOP END
.REFILL
	TRACING  is another  option.    It  allows the  user  greater
control over the  exact trajectory chosen for the move.  The path can
be traced at whatever  speed desired.   The path,  or  "parameterized
frame",  is  a specification of what  frame the arm is  to go through
for each  value of the parameter.  Of course,  one also specifies the
relation between the parameter and real time.  It is also possible to
state the  grain of the motion  and the tolerance  that is acceptable
(as a distance in 3-space). 
	An example:
.UNFILL
	MOVE YELLOW 
		TRACING CENTER + (COS(P),SIN(P),0)
		FOR P α← 0 UNTIL 2*π
		WITHIN .1;
.REFILL

should move the yellow arm in a circle around CENTER.

	The option  MAINTAINING ORIENTATION causes
the trajectory computed  by HAL to  try to  maintain the same
hand orientation  throughout  the motion.    Of  course,   the  final
orientation must be  the same as the initial orientation  for this to
work at all.

	USING lists a  set of modes under  which the motion is  to be
performed.   These can  include duration  control,  force  applied in
some direction,    which  directions  should be  free  from  position
feedback,   and what the departure  and approach should be  (if it is
desired to override the defaults,  which are properties of the frames
involved at the beginning and end of motion). 

	Duration  refers to  the  time  elapsed  since the  start  of
motion.  In an ON-monitor,  one can  check for duration  becoming too
long.  In the USING construct, duration is merely a note for how long
the trajectory should be  planned to take.  One can  use %7≥%*,=,or ≤ for
this, although ≤ is most likely risky.
Example:
.UNFILL
	USING DURATION %7≥%* 3    .
.REFILL

	Force specifies  both a direction  and an intensity.   During
the  motion,  an  attempt will be  made to apply  the required force.
This is done by  applying certain forces  in some combination of  arm
joints.  Which arm joints are affected is decided by the compiler; if
the motion  is long, it is likely that the particular joints applying
force will be scheduled to change during the motion, as the aspect of
the arm changes. To get a  force in the hand's Z direction, say,  one
would write 
.UNFILL
	FORCE = Z WRT @     ,
.REFILL
 where @ is a symbol meaning "the  location
of the controllable frame,  as continuously changing during motion." 

	A free  direction is  one in  which all  position errors  are
ignored  by the servo.   As for forces, the  compiler translates this
into a  set  of  joints  which  are to  have  the  position  feedback
disabled, and this  set may vary throughout the motion.   Once again,
the @ may be used to refer to the controllable frame as it moves.

	It  is possible to  free more  than one direction,   or apply
force in  more than  one direction.    In this  case, the  directions
specified  for force  must  be orthogonal,   as  must  the directions
specified for free.  This restriction is enforced by the  requirement
that  multiple forces  and  frees  all be  set  with respect  to  the
cardinal directions of one frame.  Examples:
.UNFILL
	USING FORCE = Z WRT FROB, FORCE=X WRT FROB, FREE=Y WRT HAND
	USING FORCE = (2,1,0), APPROACH = APPROACH(FROB);
		COMMENT:  This form will be explained in {sssref dep};
	USING FREE = X, FREE = Y, DEPARTURE = NIL
.REFILL
	Since both force and free are translated by the compiler into
special handling  of certain joints, surprises  can result from large
discrepancies between  the  planning values  and the  actual  runtime
values.   The  motions  will go  through the  right  places, but  the
directions of force and freedom may be wildly wrong.

	The notions  of force and  free are  hardware-dependent; they
depend   on  the  particular  arm   in  use.     Hopefully,  as  more
sophisticated arms become available, USING can be extended  to handle
whatever new capabilities exist.

.dep: NEWSSS DEPROACHES

	Many objects  have shapes which  necessitate care as  the arm
approaches  them or departs from  them.  HAL supplies  a method for
insuring that every  time the arm  approaches a  frame, it will  pass
through some  associated spot  first, and every  time it  leaves that
frame,   it passes once  again through the same spot.   The "spot" is
termed a DEPROACH  (from DEParture  and apPROACH),   and is really  a
transformation  to  be applied  to  the frame  involved  in order  to
discover the appropriate place through which to pass. The reason that
a transformation is used,  and not a frame itself, is that deproaches
are  often used  for large  objects, like the  table, and  the proper
point to pass through in that case is 10 centimeters above  where the
hand  is meant  to  arrive on  the  table (or  above  where the  hand
currently is  on the table,  if a departure is meant),  not the point
10 centimeters above the table origin.

	One  specifies  the  deproach  of  an  object  by  making  an
assertion; for  example, the deproach  of the table  is automatically
asserted as follows (see {sssref asr} concerning assertions):
.UNFILL
	ASSERT FACT (DEPROACH TABLE [(0,0,10) | (0,0,0)])   .
.REFILL
 This means
that the correct departure point  from a spot S on the  table will be
.UNFILL
	[(0,0,10) | (0,0,0)] * S    .
.REFILL

	As an  object moves about in space,  its deproach point moves
as well.   Thus 
.UNFILL
	ASSERT FACT (DEPROACH FROB [(0,10,0) | (0,0,0)]) 
.REFILL
means that  no matter  where  the frob  may  go, its  deproach  will be  10
centimeters  in the y-direction away from  its origin, as measured in
its own coordinate system.


	What if  the hand  is not  at FROB,   but wishes  to use  its
deproach?  This also is handled correctly; the deproach point will be
10 centimeters in FROB's y-direction from wherever the hand  actually
is.

	Deproaches,  being  transformations,  also have the  power to
include  rotations. These  are considered to  be rotations  about the
origin of  the coordinate system  involved; the  rotation occurs,  as
usual, before translation.  Thus
.UNFILL
	ASSERT FACT  (DEPROACH FROB  [(5,0,0) | (0,0,90)])  
.REFILL
has the effect  that whenever  FROB's deproach  is used  from any  frame, the
deproach frame will be  the given frame, rotated about  FROB's origin
by 90  degrees in Z,  and then translated  5 centimeters in  FROB's X
direction. 


	Suppose that a frame
F  is given  deproach transformation D.   It  is desired to  find the
frame which  is  the deproach  point from  some  other frame  H  (for
example, where the hand is, for  departure), using F's deproach.  The
frame  which  is  used  is 
.UNFILL
	F  *  D  * (Fα→TABLE)  *  H    .
.REFILL

If H = F, then the identities 
.UNFILL
	(F α→ TABLE) * F = TABLE, and X * TABLE =X     .
.REFILL
reduce the expression for F's deproach to F * D.

	When an arm moves to a frame, this is how the deproach point
is calculated:  The frames own deproach is used, if it has one.  If
not, then a search is made up the ladder of attachment (that is, frames
to which the given frame is attached are searched) until one is found
with a deproach.  
If none  at all is found,  then the table's
deproach is  used as a  default.   (One way  to think of  this is  to
consider all frames ultimately  attached to the table.) In approaching
a frame which  is  the  result of a  calculation (such as  MOVE
YELLOW TO FROB + (0,0,1)) the default deproach is null.
The default deproach of ⊗ is also null.

	In departing from a frame,  it  matters whether or not that frame is
now attached to the hand.  If not, then the same algorithm for
finding deproach is used as in the case of approach. But if the frame
has been detached  from some erstwhile mother and is  now attached to
the  hand,  then its  old mother's deproach is used  (and if there is
none, the same search is  made).  
A frame
attached to the hand still has some "memory" of its previous state of
attachment, by means of an automatic WAS_ATTACHED assertion 
which will be mentioned in {ssref att}.

	All of  the automatic generation  of deproach  points can  be
explicitly overridden in a MOVE  statement by means of the USING
clause.  This can take two forms:
.UNFILL
	USING  APPROACH = NIL;  COMMENT: No approach point  at all is used;
	USING APPROACH = DEPROACH(frob); COMMENT: frob's  deproach is used;
.REFILL

	Note that the word  APPROACH could be replaced by DEPROACH in
both of the examples above.
.NEWSSS	COMPLEX MOVES

	A complex move is  one which involves more than one  arm at a
time.  A distinction can be  made between moves  which merely require
simultaneous acquisition of "agreement points" (let us call this weak
synchrony),  and   those  which   require  true   coordinated  motion
throughout (strong synchrony).

	Weak synchrony is achieved by pairing frames to make composite
VIA points and destinations.  A paired frame  has the form: α{F1, F2α}.
Here is an example of a move statement using paired frames:
.UNFILL
	MOVE α{YELLOW, BLUEα}
		VIA α{Y1,B1α},α{Y2,*α},α{Y3,B2α}
		TO α{Y4,B3α}
		ON α{FORCE(Z)>3,*α} DO STOP
.REFILL
	The via list is  composed of a set of paired  frames, where *
indicates  "don't  care".   In  the  example  shown,  the arms  start
together, achieve  Y1 and B1  simultaneously, the  yellow arm  passes
through Y2, and together they pass through Y3 and B2.  

	It  is  now  more  cumbersome  to  specify  ON  monitors,  or
conditions  in general.   The  paired construct  applies for  all the
optional fields; thus, one can write
.UNFILL
	USING FORCE=α{3*Z,(2*Y) WRT @α}
.REFILL
  to get the  yellow arm applying  a
force of strength 3*Z, and the blue one to have a force of strength 2*Y
in the coordinate system of the blue hand.

	The meaning of ⊗ and @ is  now relative to which side of  the
pair they occupy; in  the example above, the left  side always refers
to the yellow arm,  and the right side to the blue.  To override this
convention, one may use expressions like "@.YELLOW", or "⊗.BLUE".

	The meaning of STOP in the example above is extended  to both
arms at once;  in order to specify  only one, it is  necessary to say
"STOP YELLOW" or "STOP BLUE".

	Strong synchrony involves one concept not included above: The
ability to  specify the location of one  arm throughout the motion in
terms of the location of the  other arm.  The construct which  allows
this  specification  is  COORDINATING;  it allows  one  to  give  an
expression for  the location of one of the two arms.  Suppose we wish
to keep both arms in "lockstep", that is, the  blue arm should retain
its relative position to the yellow arm throughout the motion.  (This
might be necessary for lifting some object by its two ends.) One  way
to code this task is as follows:
.UNFILL
	MOVE α{YELLOW, BLUEα}
		TO α{Y1, *α}
		VIA α{YA,BAα},α{YB,BBα}
		COORDINATING LOC.BLUE = LOC.YELLOW + ⊗.BLUE - ⊗.YELLOW
		USING FREE = α{*, @.YELLOW - @.BLUEα}
		α{MAINTAINING ORIENT, MAINTAINING ORIENTα}

.REFILL
.NEWSSS SEARCHES

	A  SEARCH is  very  much like  a  move.   It  is a  means  of
specifying  repeated action  in a  spiral.  As  with a  MOVE,   it is
necessary to name a controllable frame which is to be moved.   The ON
construct is exactly as for MOVEs.

	One must  stipulate what the  plane of  the search is  to be.
This is  accomplished in either of two ways: ACROSS <plane> means the
search is to take place in the plane specified.   If the controllable
frame (say, the hand) is in  fact not in that plane at the start, then
the plane parallel to the given one through the hand will be used. It
is assumed  that the  hand begins at  the center  of the search.  The
other  alternative is to say  NORMAL_TO <vector>.   This,
together with the location of the hand,
 will specify
the plane you want for the search.

	The size of the increment is specified in a  USING construct.
An example is USING INCREMENT = 3.

	The servo does almost all the calculating for searches; it is
fed the normal direction and the increment size.

	Most important  for the search is the REPEATING construct, which
specifies what motion is to  be performed at each
iteration.  It is advisable that  the motion cause the
arm to return to the point at which it began, in order to assume  the
same plane at the onset of each iteration.  If this is not done, then
the  servo will  move  it back  each time  anyway.   When  the search
succeeds (and it  is the  duty of  the user to  specify what  success
means for each search) the search  can be terminated in either of two
ways: by  setting a flag in the REPEATING and checking it with an ON,
or by  using the  construct TERMINATE inside  the REPEATING  at the  right
place.
	Here is a complete example:
.UNFILL
	SEARCH YELLOW
		ACROSS P1
		REPEATING 
			BEGIN
			FRAME SET;
			SET α← YELLOW;
			MOVE YELLOW TO ⊗-Z
				ON FORCE(Z) > 3 DO TERMINATE;
			MOVE YELLOW TO SET DIRECTLY;
			END
.REFILL

.NEWSSS	CENTER

	Occasionally the hand is positioned around an object, but it
is not certain if it is centered.  One wants to close the fingers
slowly, moving the arm meanwhile to accomodate to the location of
the object.  
This  is accomplished by means of the CENTER command.
The direction that the hand will move is the direction between its
fingers.  All that the CENTER command needs is the name of the
arm being moved.
The use of  ON is just  as in  a
search or any other motion.

	Here is a simple example:
.UNFILL
	CENTER BLUE
		ON SQUEEZE > 4 DO STOP
.REFILL
Note that this is command, unlike MOVE, treats the fingers and
the arm together as one device.

.NEWSSS	CONSTANT VELOCITY MOTION

	A special form of the MOVE instruction is provided
to cause  the arm to  quickly achieve a  particular velocity,
and to hold it in straight-line motion for a given distance:
.UNFILL
	MOVE YELLOW
		VELOCITY=V1
		THROUGH T
		FOR DISTANCE=4		.
.REFILL
The VELOCITY clause tells which  vector to follow,  and how  fast. The
THROUGH clause tells the compiler where the move  expects to end. The FOR
DISTANCE tells the maximum distance the hand should go. It is assumed
that such a move will normally terminate by an ON test.
For example:
.UNFILL
	MOVE YELLOW
		WITH VELOCITY=V1
		THROUGH T
		FOR DISTANCE=4
		ON FORCE(V1) > 2 DO STOP
.REFILL

.NEWSSS DEVICE CONTROL

	Generally, an arm will  stop its motion when it  has achieved
its destination.   Often it is necessary  to stop it prematurely, for
example, if some  error condition  is detected.   The statement  STOP
YELLOW causes the  yellow arm to be unconditionally  stopped, and any
motion statement operating it will terminate. Each device has a name,
and can be  stopped by name.   Currently, the legal device  names are
YELLOW,  BLUE,   VICE,  DRIVER (an  electric  screwdriver), YFINGERS,
BFINGERS (The fingers of the two arms). STOP without any device  name
is only legal within a motion command;  it stops whatever device(s) that
command is operating.

	There is a  general command for operating devices  other than
arms; it is hoped that this will be flexible enough for any device we
are likely to use (if not, we will add special new forms).  Assume we
have the device TURNTABLE, which is capable of moving at any velocity
and  for any length of time, but which  cannot go to a particular set
point.  Then the syntax would be this:
.UNFILL
	OPERATE TURNTABLE
		WITH VELOCITY=3
		WITH DURATION=8
.REFILL
The idea is that  the WITH construct will suffice to  account for any
special  data (in this case,  velocity and duration)  peculiar to the
particular  device.    The  OPERATE  statement  also  allows  the  ON
construct, so it can test for special conditions and take appropriate
actions.

	The screwdriver is a  hand-held device which can be  run at a
range  of speeds,  in either  direction.   By convention,  a positive
velocity   means   clockwise,   and   a   negative   velocity   means
anticlockwise.  The  relevant reserved  word  is  VELOCITY, which  is
equated  with the name  of a  scalar variable, which  will be queried
each time  the  screwdriver  servo wakes  up  to determine  how  much
voltage  to apply to the  motor.   This allows  the velocity  to change
during the operation of  the device, perhaps under  the control of  a
parallel process which is monitoring some conditions.
	An example:
.UNFILL
	OPERATE DRIVER
		WITH VELOCITY=SP
		ON DURATION>4 DO STOP
		ON DURATION>2 DO SP α← 2*SP

.REFILL

	Each arm  has two  fingers at  the end  which are capable  of
closing and  opening at various speeds.   The relevant reserved words
are OPENING, which is to be set to the desired (scalar) opening,  and
VELOCITY, which is to be set to the speed desired.  It is possible to
refer to the scalar variable SQUEEZE, which indicates the force being
applied by the fingers.
	An example:
.UNFILL
	OPERATE FINGERS
		WITH OPENING=4
		WITH VELOCITY=2
		ON SQUEEZE > 3 DO STOP
.REFILL